#include <iostream>
#include <ros/ros.h>
#include <birl_manipulator/InverseKine.h>
#include <birl_manipulator/TargetPose.h>



int main(int argc,char* argv[])
{
    std::string user_answer;
    ros::init(argc,argv,"manipulator_ik_client");
    if(argc!=7)
    {
        ROS_INFO("usage: ik_client x y z w p r");
        return 1;
    }
    
    ros::NodeHandle node_handle;
    ros::ServiceClient client = node_handle.serviceClient<birl_manipulator::InverseKine>("inversekine");
    ros::ServiceClient move_robot_client = node_handle.serviceClient<birl_manipulator::TargetPose>("manipulator_position_service");


    birl_manipulator::InverseKine srv;
    static double zero_joint_state[5]={0,90,90,0,0}; //机械矫正零点相对于绝对零点的角度

    // 当前关节角度默认为相对于绝对零点的角度
    srv.request.current_joint_state.clear();
    for(int i=0;i<5;i++)
    {
        srv.request.current_joint_state.push_back(zero_joint_state[i]);
    }

    srv.request.descartes_pos_commands.clear();
    for(int i=1;i<7;i++)
    {
        srv.request.descartes_pos_commands.push_back(atof(argv[i]));
    }
    if(client.call(srv)){
        if(srv.response.ifGetSolve){
            ROS_INFO("Get the Ik solution : ");
            ROS_INFO_STREAM(" "<<srv.response.joint_pos_commands[0]<<" "
                            " "<<srv.response.joint_pos_commands[1]<<" "
                            " "<<srv.response.joint_pos_commands[2]<<" "
                            " "<<srv.response.joint_pos_commands[3]<<" "
                            " "<<srv.response.joint_pos_commands[4]<<" ");
        std::cout<<"Move the Robot or not?(Yes or No):";
        std::cin>>user_answer;
        if(user_answer.compare("Yes")==0){
            birl_manipulator::TargetPose move_robot_srv;
            for(int i=0;i<5;i++){
                move_robot_srv.request.target_pose[i]=srv.response.joint_pos_commands[i];
            }
            move_robot_client.call(move_robot_srv);
        }
        else{
            return 0;
        }
        }
    }
    else{
        ROS_ERROR("Failed to call service ik_server");
        return 1;
    }
    return 0;

}